#include "process_depth_data.h"

#if 1
ProcessData::ProcessData()
{

}

ProcessData::~ProcessData()
{
    close_device_orbbec();
    
}

int ProcessData::open_device_orbbec()
{
    stat_ = OpenNI::initialize();
	if (stat_ != STATUS_OK) {
		printf("Initialize failed\n%s\n", OpenNI::getExtendedError());
	}

    stat_ = device.open(ANY_DEVICE);
    if (stat_ != STATUS_OK) {
		printf("Couldn't open device\n%s\n", OpenNI::getExtendedError());
		return 2;
	}

    if (device.getSensorInfo(SENSOR_DEPTH) != NULL) {
		//cout<<device.getSensorInfo(SENSOR_DEPTH)<<endl;
		stat_ = depth.create(device, SENSOR_DEPTH);
		if (stat_ != STATUS_OK) {
			printf("Couldn't create depth stream\n%s\n", OpenNI::getExtendedError());
			return 3;
		}
	}

	stat_ = depth.start();
	if (stat_ != STATUS_OK) {
		printf("Couldn't start the depth stream\n%s\n", OpenNI::getExtendedError());
		return 4;
	}
}

int ProcessData::close_device_orbbec()
{
	depth.stop();
	depth.destroy();
	device.close();
    OpenNI::shutdown();
}

void ProcessData::get_camera_params_orbbec()
{
    OBCameraParams cameraParam;
    int dataSize = sizeof(cameraParam);
    memset(&cameraParam, 0, sizeof(cameraParam));
    openni::Status rc = device.getProperty(openni::OBEXTENSION_ID_CAM_PARAMS, (uint8_t *)&cameraParam, &dataSize);
    if (rc != openni::STATUS_OK) {
        std::cout << "Error:" << openni::OpenNI::getExtendedError() << std::endl;
        return;
    }
    cam_para.fx = cameraParam.l_intr_p[0]; 
    cam_para.fy = cameraParam.l_intr_p[1]; 
    cam_para.cx = cameraParam.l_intr_p[2];
    cam_para.cy = cameraParam.l_intr_p[3];
    std::cout<<"cam_para.fx: "<<cam_para.fx<<", cam_para.fy: "<<cam_para.fy<<std::endl;
    std::cout<<"cam_para.cx: "<<cam_para.cx<<", cam_para.cy: "<<cam_para.cy<<std::endl;
}

int ProcessData::start_process_orbbec(cv::Mat &depth_mat)
{
    int changedStreamDummy;
    VideoStream* pStream = &depth;
	stat_ = OpenNI::waitForAnyStream(&pStream, 1, &changedStreamDummy, SAMPLE_READ_WAIT_TIMEOUT);
	if (stat_ != STATUS_OK) {
		printf("Wait failed! (timeout is %d ms)\n%s\n", SAMPLE_READ_WAIT_TIMEOUT, OpenNI::getExtendedError());
	}

	stat_ = depth.readFrame(&frame);
	if (stat_ != STATUS_OK) {
		printf("Read failed!\n%s\n", OpenNI::getExtendedError());
	}

	if (frame.getVideoMode().getPixelFormat() != PIXEL_FORMAT_DEPTH_1_MM &&
		frame.getVideoMode().getPixelFormat() != PIXEL_FORMAT_DEPTH_100_UM) {
		printf("Unexpected frame format\n");
	}
    const int w = frame.getWidth();
	const int h = frame.getHeight();
	cv::Mat img_depth(cv::Size(w, h), CV_16UC1, (DepthPixel*)frame.getData(), cv::Mat::AUTO_STEP);
    img_depth.copyTo(depth_mat);
    //process_depth_data(img_depth);
}
#endif

int ProcessData::process_depth_data(Mat &depth)
{
    Mat dep, dep_mask, dep_src, dep_mat;
    cv::Mat dep_angle(depth.rows, depth.cols, depth.type(), Scalar::all(0));

	double time_start = static_cast<double>(getTickCount());
	depth.copyTo(dep_src);
	depth.copyTo(dep_mat);

    point_angle_trans(depth, 0, 600, 2000);
	//double time = (static_cast<double>(getTickCount()) - time_start) / getTickFrequency();
	//std::cout<<"time1: "<<time<<std::endl;

    get_threshold_mask(depth, dep, 300, 1000);//0.2m~1.0m
    get_threshold_mask(depth, dep_mask, 1000, 1800);//1.0m~1.8m

    int scale = 10;
    filer_noise_test(dep, 10, 0.7);//(20 or 30)
    filer_noise_test(dep_mask, 10, 0.65);
	cv::add(dep, dep_mask, dep);
    dep.convertTo(dep, CV_8UC1, 1.0 / scale);
    Mat out = dep;

    Mat color = gray_to_color(out);
    const uchar max_dist_const = 250;//2.5m
    int xindex, yindex, i, j, x1, x2, y1, y2;
	int detect_num = 0;
	double x_distance = 0;

    for (xindex=0; xindex<X_ZONE_NUM; xindex++) {
        //get_range(xindex, &x1, &x2, &y1, &y2);
		get_xrange(xindex, &x1, &x2);
        uchar max_dist = 0, min_dist = max_dist_const; //2.5m

		for(yindex=0; yindex<Y_ZONE_NUM; yindex++) {
			get_yrange(yindex, &y1, &y2);

	    	for( i = y1; i< y2; i+=Y_DIVIDE) {
            	const uchar *cur = out.ptr<uchar>(i);
				ushort* src_value_ptr = dep_mat.ptr<ushort>(i);
            	for (j=x1; j<x2; j+=X_DIVIDE) {
		        	uchar val = cur[j];
                	//ushort src_value = dep_mat.at<ushort>(i, j) / scale;

		        	if(val > 20 && val < max_dist_const){ // >20cm is the valid data
		            	if(val> max_dist) {
			            	max_dist = val;
		            	} else if(val < min_dist) {
			            	min_dist = val;
            				x_distance = (j - cam_para.cx) * (src_value_ptr[j]/scale) / cam_para.fx;
		            	}
		        	}
	        	}
	   		}
        	if (min_dist > max_dist_const) {
            	mix_pack.info_pack[detect_num].min_depth = 2500; //比最大值大的范围
				
        	} else {
            	mix_pack.info_pack[detect_num].min_depth = min_dist*scale;
            	mix_pack.info_pack[detect_num].x_distance = x_distance*scale;
	    	}
			//cout<<"detect_num: "<<detect_num<<", min_dist: "<<(int)min_dist<<", x_distance: "<<(int)x_distance<<endl;
			detect_num++;

#if Display_Mode
    		cv::line(color, Point(x1, y1), Point(x1, y2), (255, 0, 255));
    		cv::line(color, Point(0, y1), Point(frame_width, y1), (255, 0, 255));
        	float minDist = min_dist*0.01;
        	char strDist[128]={0};
 	    	sprintf(strDist,"%.2fm",minDist);
	    	std::string text = strDist;
        	int font_face = cv::FONT_HERSHEY_COMPLEX; 
        	double font_scale = 0.7;
        	int thickness = 1;
        	cv::Point origin; 
        	origin.x = x1;
	    	origin.y = y1;
	    	//cv::putText(color, text, origin, font_face, font_scale, cv::Scalar(255, 255, 255), thickness, 1, 0);
#endif
		}
    }

#if Display_Mode
    imshow("depth RGB", color);
    cv::convertScaleAbs(dep_src, depth_view, 0.03, 0);
    cv::applyColorMap(depth_view, depth_view, cv::COLORMAP_JET);
    cv::imshow("depth_view", depth_view);
    waitKey(1);
#endif

    return 0;
}

bool ProcessData::point_angle_trans(cv::Mat& src, int angle, int height, int height_max)
{
    cv::Mat src_angle(src.rows, src.cols, src.type(), Scalar::all(0));
    cv::Mat src_fall(src.rows, src.cols, src.type(), Scalar::all(0));
	double angle_h = angle * 3.1415926 / 180;
	int height_m = height_max - height;
	double cos_angle = cos(angle_h);
	double sin_angle = sin(angle_h);
	double cam_para_yb = 1.0 / cam_para.fy;

    for (int i = 0; i < src.rows; i++) {
		ushort* val_dep_ptr = src.ptr<ushort>(i);
		ushort* val_angle_ptr = src_angle.ptr<ushort>(i);
		ushort* val_fall_ptr = src_fall.ptr<ushort>(i);
        for (int j = 0; j < src.cols; j++) {
			ushort val_dep = val_dep_ptr[j];

            if (val_dep > 4000 || val_dep < 200) {
        	    continue;
		    }

			double y_val = (i - cam_para.cy) * val_dep * cam_para_yb;
			double y_val_trans = y_val * cos_angle + val_dep * sin_angle;

			if(y_val_trans > height + 360) {  // test
				val_fall_ptr[j] = val_dep;
				continue;
			}

			if(y_val_trans > height - 80 || y_val_trans < -height_m) { //|| y_val_trans < -1000) 120
				//src.at<ushort>(i, j) = 0;
				continue;
			}

			double val_trans = val_dep * cos_angle - y_val * sin_angle;
			val_angle_ptr[j] = (ushort)val_trans;
        }
    }
    int count = filer_noise_test(src_fall, 20, 0.7);//(20 or 30)
    src_angle.copyTo(src);

    if(count > 40) {
        printf("filer_noise_test: %d\n", count);
        return true;
    }
    return false;
}

void ProcessData::get_threshold_mask(Mat &src, Mat& thr, int thr_min, int thr_max)
{
	src.copyTo(thr);
    for (int i = 0; i < src.rows; i++) {
		ushort* val_dep_ptr = thr.ptr<ushort>(i);
        for (int j = 0; j < src.cols; j++) {
			ushort val_dep = val_dep_ptr[j];
            if (val_dep > thr_max || val_dep <= thr_min){
            	//thr.at<ushort>(i, j) = 0;
				val_dep_ptr[j] = 0;
			}
        }
    }
}

int ProcessData::filer_noise_test(Mat &src, int element_size, double val_prop)
{
    int element_half = element_size / 2;
    int step_num = element_size;
    int count = 0;

    for(int i = element_half; i <= src.rows - element_half; i = i + step_num){
        for(int j = element_half; j <= src.cols - element_half; j = j + step_num){
            if(j == 1245 && element_size == 30) {
                j = j - 10;
            }

			cv::Rect rect;
			rect.x = j - element_half;
			rect.y = i - element_half;
			rect.width = 2 * element_half;
			rect.height = 2 * element_half;

			int val_num_eff = cv::countNonZero(src(rect));
			int val_num_all = rect.width * rect.height;
			double val_prop_now = val_num_eff * 1.0 / val_num_all;

			cv::Mat all_zero(rect.height, rect.width, src.type(), cv::Scalar::all(0));
			cv::Mat tmp_mat = src(rect);
            if(val_prop_now < val_prop){
				all_zero.copyTo(tmp_mat);
            }
            else {
                count++;
            }
        }
    }

    return count;
}

cv::Mat ProcessData::gray_to_color(Mat &gray)
{
    Mat dispRgb(gray.rows, gray.cols, CV_8UC4);
    Vec4b red(0, 0, 255), green(0, 255, 0), blue(255, 0, 0), purple(255, 0, 255),black(0, 0, 0), yellow(0, 255, 255), white(255, 255, 255), cyan(255, 255, 0), cont(125, 0, 125);
    for(int i = 0; i < gray.rows; i++){
        for(int j = 0; j < gray.cols; j++){
            uchar grayVal = gray.at<uchar>(i,j);
	        if(grayVal==0 || grayVal > 400){
 		        dispRgb.at<Vec4b>(i,j) =black;
	        } else if(grayVal< WARN_LIMIT*0.5 * 0.1){
 		        dispRgb.at<Vec4b>(i,j) =red;
	        }else if(grayVal< WARN_LIMIT*0.8* 0.1){
                dispRgb.at<Vec4b>(i,j) =yellow;
            }else if(grayVal< WARN_LIMIT*1.1 * 0.1){
                dispRgb.at<Vec4b>(i,j) =green;
            }else if(grayVal< WARN_LIMIT*1.4 * 0.1){
                dispRgb.at<Vec4b>(i,j) =blue;
            }else if(grayVal< WARN_LIMIT*1.8 * 0.1){
                dispRgb.at<Vec4b>(i,j) = purple;
            }else if(grayVal < WARN_LIMIT*2.2 * 0.1){
                dispRgb.at<Vec4b>(i,j) = cyan;
            }else if(grayVal < WARN_LIMIT*2.5 * 0.1){
                dispRgb.at<Vec4b>(i,j) = cont;
            }else if(grayVal > WARN_LIMIT*2.5 * 0.1){
                dispRgb.at<Vec4b>(i,j) = white;
            }
	    }
    }
    // imshow("depth RGB", dispRgb);
    return dispRgb;
}

void ProcessData::get_xrange(int xindex, int *x1, int *x2)
{
    int zone_width = frame_width / X_ZONE_NUM;
    *x1 = xindex * zone_width;
    *x2 = (xindex+1) * zone_width;
}

void ProcessData::get_yrange(int yindex, int *y1, int *y2)
{
    int zone_height = frame_height / Y_ZONE_NUM;
    *y1 = yindex * zone_height;
    *y2 = (yindex+1) * zone_height;
}

